#include <vector>
#include <algorithm>
#include <ros/ros.h>
#include <cmath>
#include <math.h>
#include <deque>
#include <mutex>
#include <thread>
#include <fstream>
#include <csignal>

#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMavFrame.h>

#include <geometry_msgs/TwistStamped.h>
 
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/Bool.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Imu.h> 
#include <std_msgs/Float32MultiArray.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// #include <sensor_msgs/LaserScan.h>  qaq
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Altitude.h>
#include <mavros_msgs/ActuatorControl.h>
#include <mavros_msgs/WaypointList.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/common.h>
#include <pcl/filters/voxel_grid.h>

#include <unionsys_core.hpp>

#include <visualization_msgs/Marker.h>


#include <swarm_msgs/BoundingBoxes.h>
#include <swarm_msgs/BoundingBox.h>

RegisterMSN *unionsys_core = new RegisterMSN();

const float deg2rad = 3.1415926535798/180.0;
const float rad2deg = 180.0/3.1415926535798;

int image_width;
int image_height;
struct Box
{
	float x_min;
	float y_min;
	float z_min;
	float x_max;
	float y_max;
	float z_max;
	int id;
};

//ros node define
ros::Subscriber local_odom_sub,LIO_odom_sub, laser_map_sub;
ros::Subscriber state_sub;
ros::Subscriber map_cloud_sub,lio_map_sub;
ros::Subscriber human_yolo_sub, car_yolo_sub, oil_yolo_sub, weapon_yolo_sub, bigarmor_yolo_sub, shatanche_yolo_sub;

ros::Publisher  marker_pub, target_pub,  marker_id_pub;
ros::Publisher  human_pose_pub, car_pose_pub, oil_pose_pub, weapon_pose_pub, bigarmor_pose_pub, shatanche_pose_pub;


ros::Timer  cluster_timer, pointcloud_proj_timer, publish_target_timer;

//msg define
mavros_msgs::State current_state;
geometry_msgs::Point current_point;
geometry_msgs::Quaternion current_angle;
geometry_msgs::Vector3 curr_plane_angle;
Eigen::Quaterniond current_angle_Q;
std::vector<geometry_msgs::Point> human_list;
std::vector<geometry_msgs::Point> car_list;
std::vector<geometry_msgs::Point> oil_list;
std::vector<geometry_msgs::Point> weapon_list;

std::vector<geometry_msgs::Point> bigarmor_list;
std::vector<geometry_msgs::Point> shatanche_list;

int pub_human_cnt = 0;
int pub_oil_cnt = 0;
int pub_car_cnt = 0;
int pub_weapon_cnt = 0;
int pub_bigarmor_cnt = 0;
int pub_shatanche_cnt = 0;
int clusterId = 0;
double map_save_last = 0.0;


template <typename T>
T limit(T a, T max)
{
  if(a > max)
    a = max;
  if(a < -max)
    a = -max;
  return a;
}

std::vector<Box> deal_detection_to_3dpose(swarm_msgs::BoundingBoxes msg, std::string class_name, double real_size,int id_zone);

void draw_box(Box temp_box, int id, double width, double red, double green, double blue);
void draw_name(Box temp_box,std::string name, int id);

